/**
 * @file EulerAngles.cpp
 * @author  Aljosa Osep <aljosa.osep@gmail.com, Fletcher Dunn, Ian Parnberry>
 * @version 1.0
 *
 * @section LICENSE
 *
 * Redistribution and use in source and binary forms, with or without modification, are
 * permitted provided that the following conditions are met:

 *  1. Redistributions of source code must retain the above copyright notice, this list of
 *    conditions and the following disclaimer.

 *  2. Redistributions in binary form must reproduce the above copyright notice, this list
 *     of conditions and the following disclaimer in the documentation and/or other materials
 *     provided with the distribution.

 * THIS SOFTWARE IS PROVIDED BY <Aljosa Osep> ``AS IS'' AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL <Aljosa Osep>> OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 * The views and conclusions contained in the software and documentation are those of the
 * authors and should not be interpreted as representing official policies, either expressed
 * or implied, of <Aljosa Osep>.
 *
 * @section DESCRIPTION
 *
 * EulerAngles implementation
 * Based on examples from 3D Math Premier of Graphics and Games Development book
 * Original authors: Fletcher Dunn, Ian Parnberry. Thanks!
 */

#include "MathUtil.h"
#include "EulerAngles.h"
#include "Quaternion.h"

namespace xEngine
{
    namespace PlatformIndependent
    {
        namespace Math
        {

            // Constructors

            /**
             * Default constructor
             *
             */
            EulerAngles::EulerAngles() { // Default - set angles to 0.0f
                this->heading = this->bank = this->pitch = 0.0f;
            }

            /**
             * Setter constructor
             *
             */
            EulerAngles::EulerAngles(float h, float p, float b) : heading(h), pitch(p), bank(b) { }

            // Operations
            /**
             * <A short one line description>
             *
             * <Longer description>
             * <May span multiple lines or paragraphs as needed>
             *
             * @param  Description of method's or function's input parameter
             * @param  ...
             * @return Description of the return value
             */
            void EulerAngles::identity() { // Set angles to 0
                this->heading = this->bank = this->pitch = 0.0f;
            }

            /**
             * Canonize
             *
             * Sets angles to canonical form
             *
             */
            void EulerAngles::canonize() { // Angles canonical form
                // 1. pitch in range -pi ... pi
                pitch = wrapPi(pitch);

                // 2. check back side
                // Canonical range: -pi/2 ... pi/2
                if (pitch < -kPiOver2) { // if pitch < -PI/2
                    pitch = -kPi - pitch;
                    heading += kPi;
                    bank += kPi;
                }
                else if (pitch > kPiOver2) { // If pitch > kPiOver2
                    pitch = kPi - pitch;
                    heading += kPi;
                    bank += kPi;
                }

                // Check gimbel lock
                if (fabs(pitch) > kPiOver2 - 1e-4) {
                    // We are in the gimbel lock
                    // Assign all rotation about vertical axis to heading

                    heading += bank;
                    bank = 0.0f;
                }
                else
                    // Just wrap bank angle (canonical range)
                    bank = wrapPi(bank);

                // Wrap heading to canonical range
                heading = wrapPi(heading);
            }

            /**
             * From Object To Inertial Quaternion
             *
             * Convert quaterion to Euler angle format; both object-to-inertial
             *
             * @param  An quaternion
             */
            void EulerAngles::fromObjectToInertialQuaternion(const xEngine::PlatformIndependent::Math::Quaternion &q) {
                // pitch = asin (-2(yz+wx))
                float sinpitch = -2.0f * (q.y * q.z + q.w * q.x);

                // Check for gimbel lock with tolerance
                if (fabs(sinpitch) > 0.9999f) {
                    pitch = kPiOver2 * sinpitch;

                    // Compute heading, bank to 0ro
                    heading = atan2(-q.x*q.z + q.w*q.y, 0.5f - q.y*q.y - q.z*q.z);
                    bank = 0.0f;
                }
                else {
                    // Compute angles.
                    // Angles were already validated.

                    // Pitch
                    pitch = asin(sinpitch);

                    // Heading = atan2(xz - wy, 1/2 - x^2 - y^2)
                    heading = atan2(q.x*q.z + q.w*q.y, 0.5f - q.x*q.x - q.y*q.y);

                    //Bank = atan2(xy - wz, 1/2 - x^2 - z^2)
                    bank = atan2(q.x*q.y + q.w*q.z, 0.5f - q.x*q.x - q.z*q.z);
                }
            }

            /**
             * From Inertial To Object Quaternion
             *
             * Convert quaterion to Euler angle format; inertial-to-object
             *
             * @param  An quaternion
             */
            void EulerAngles::fromInertialToObject(const Quaternion &q) {
                // pitch = asin (-2(yz+wx))
                float sinpitch = -2.0f * (q.y * q.z + q.w * q.x);

                // Check for gimbel lock with tolerance
                if (fabs(sinpitch) > 0.9999f) {
                    pitch = kPiOver2 * sinpitch;

                    // Compute heading, bank to 0ro
                    heading = atan2(-q.x*q.z - q.w*q.y, 0.5f - q.y*q.y - q.z*q.z);
                    bank = 0.0f;
                }
                else {
                    // Compute angles.
                    // Angles were already validated.

                    // Pitch
                    pitch = asin(sinpitch);

                    // Heading = atan2(xz - wy, 1/2 - x^2 - y^2)
                    heading = atan2(q.x*q.z - q.w*q.y, 0.5f - q.x*q.x - q.y*q.y);

                    //Bank = atan2(xy - wz, 1/2 - x^2 - z^2)
                    bank = atan2(q.x*q.y - q.w*q.z, 0.5f - q.x*q.x - q.z*q.z);
                }
            }
        }
    }
}

 // Matrix convertation not covered yet; TODO?

